#include "sin_fit.h"

#include <cstdio>
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif


// Calculate the model. Y = a * sin(x + b) + c;
int sin_f(const gsl_vector * x, void * data, gsl_vector * f)
{
     size_t n = ((struct Data *)data)->n;
     double * y = ((struct Data *)data)->y;
     double * sigma = ((struct Data *)data)->sigma;

     double a = gsl_vector_get(x, 0);
     double b = gsl_vector_get(x, 1);
     double c = gsl_vector_get(x, 2);

     for (unsigned int i = 0; i < n; ++i)
     {
          // Model Yi = a * sin(x + b) + c;
          double t = (double)i * 2 * M_PI * 2.0 / (double)n;
          double Yi = a * sin(t + b) + c;
          gsl_vector_set(f, i, (Yi - y[i]) / sigma[i]);
     }

     return GSL_SUCCESS;
}

// Jacobian of the model.
int sin_df(const gsl_vector * x, void * data, gsl_matrix * J)
{
     size_t n = ((struct Data *)data)->n;
     double * sigma = ((struct Data *)data)->sigma;

     double a = gsl_vector_get(x, 0);
     double b = gsl_vector_get(x, 1);

     for (unsigned int i = 0; i < n; ++i)
     {
          // Jacobian matrix J(i, j) = dfi / dxj
          // where fi = (Yi - yi) / sigma[i],
          //       Yi = a * sin(x + b) + c
          // and the xj are the parameters (a, b, c)
          double t = (double)i * 2 * M_PI * 2.0 / (double)n;
          double s = sigma[i];

          gsl_matrix_set(J, i, 0, sin(t + b) / s);               // wrt a
          gsl_matrix_set(J, i, 1, a * cos(t + b) / s);           // wrt b
          gsl_matrix_set(J, i, 2, 1.0 / s);                      // wrt c
     }
     return GSL_SUCCESS;
}

int sin_fdf(const gsl_vector * x, void * data, gsl_vector * f, gsl_matrix * J)
{
     sin_f(x, data, f);
     sin_df(x, data, J);

     return GSL_SUCCESS;
}
